#include "math_curve_fitting.h"
#include <math.h>

void circle_fitting_init(circle_fitter_t* circle_fitter, float* x_array, float* y_array, int lens)
{
    circle_fitter->p_x  = x_array;
    circle_fitter->p_y  = y_array;
    circle_fitter->lens = lens;

    circle_fitter->n = (float)lens;
    circle_fitter->sum_X1 = 0.0f;
    circle_fitter->sum_X1Y1 = 0.0f;
    circle_fitter->sum_X2 = 0.0f;
    circle_fitter->sum_X2Y1 = 0.0f;
    circle_fitter->sum_X3 = 0.0f;
    circle_fitter->sum_Y1 = 0.0f;
    circle_fitter->sum_Y2 = 0.0f;
    circle_fitter->sum_Y2X1 = 0.0f;
    circle_fitter->sum_X2_Y2 = 0.0f;
    circle_fitter->sum_Y3 = 0.0f;
}

void circle_fitting_with_points(circle_fitter_t* circle_fitter, circle_t* circle)
{
    float a = 0.0f, b = 0.0f, c = 0.0f;
    float CC = 0.0f, D = 0.0f, E = 0.0f, G = 0.0f, H = 0.0f;
    float n = circle_fitter->n;

    for(int i = 0u; i < circle_fitter->lens; i++)
    {
        float x_i = circle_fitter->p_x[i];
        float y_i = circle_fitter->p_y[i];

        float X1 = x_i;
        float X1Y1 = X1 * y_i;
        float X2 = X1 * x_i;
        float X2Y1 = X2 * y_i;
        float X3 = X2 * x_i;
        float Y1 = y_i;
        float Y2 = Y1 * y_i;
        float Y2X1 = Y2 * x_i;
        float X2_Y2 = X2+Y2;
        float Y3 = Y2 * y_i;

        circle_fitter->sum_X1 += X1;
        circle_fitter->sum_X1Y1 += X1Y1;
        circle_fitter->sum_X2 += X2;
        circle_fitter->sum_X2Y1 += X2Y1;
        circle_fitter->sum_X3 += X3;
        circle_fitter->sum_Y1 += Y1;
        circle_fitter->sum_Y2 += Y2;
        circle_fitter->sum_Y2X1 += Y2X1;
        circle_fitter->sum_X2_Y2 += X2_Y2;
        circle_fitter->sum_Y3 += Y3;   
    }

    CC = n * circle_fitter->sum_X2 - circle_fitter->sum_X1 * circle_fitter->sum_X1;
    D = n * circle_fitter->sum_X1Y1 - circle_fitter->sum_X1 * circle_fitter->sum_Y1;
    E = n * circle_fitter->sum_X3 + n * circle_fitter->sum_Y2X1 - circle_fitter->sum_X2_Y2 * circle_fitter->sum_X1;
    G = n * circle_fitter->sum_Y2 - circle_fitter->sum_Y1 * circle_fitter->sum_Y1;
    H = n * circle_fitter->sum_X2Y1 + n * circle_fitter->sum_Y3 - circle_fitter->sum_X2_Y2 * circle_fitter->sum_Y1;

    a = (H * D - E * G) / (CC * G - D * D);
    b = (H * CC - E * D) / (D * D - G * CC);
    c = -1.0f * (circle_fitter->sum_X2_Y2 + a * circle_fitter->sum_X1 + b * circle_fitter->sum_Y1) / n;

    circle->x = a / (-2.0f);
    circle->y = b / (-2.0f);
    circle->radius = 0.5f * sqrt(a * a + b * b - 4.0f * c);
}


/************************ End of File **************************/
